// #define BOARD_TYPE_D4        // 定义主板型号
#define SERIAL_LOG true      // 是否从串口打印运转信息
#define LOG_INTERVAL 3000    // 打印信息的间隔 （ms）
#define AUTO_BREAK_TICKS 1000 // 无任务传入时自动停机时间 (ms)
#define OLED_FPS 60          // OLED屏幕刷新率
#define SERVO_CHECK_INTERVEL 1500 // 查询舵机运行状态间隔 (ms)

// #define USE_WIFI

#include "PinMap.h"
#include "FashionStar_UartServoProtocol.h"
#include "FashionStar_UartServo.h" 
#include "StepMotor.h"
#include "ServoCtrl.h"
#include <Wire.h>
#include "ina226.h"
#include "state_led.h"

#if defined(USE_WIFI)
#define COMMUNICATE_PORT 8888
const char *APssid = "UmekoLab";
const char *APpassword = "UmekoLab";
#endif

// -----------------舵机定义--------------------
#define SERVO_ID_F 0 //舵机ID号
#define SERVO_ID_S 1 //舵机ID号
FSUS_Protocol protocol(115200); //协议
FSUS_Servo uservo0(SERVO_ID_F, &protocol); // 创建舵机0
FSUS_Servo uservo1(SERVO_ID_S, &protocol); // 创建舵机1

// -----------------步进电机定义--------------------
StepMotor motor_0(/* pul=*/ PUL_1, /* dir=*/ DIR_1, /* channel=*/ 0);
StepMotor motor_1(/* pul=*/ PUL_2, /* dir=*/ DIR_2, /* channel=*/ 1);
LED_CONTRLLER led_controller(LED_PIN, 4);

// ----------------全局变量定义---------------------
TickType_t LastIncomeMessageTime = xTaskGetTickCount();
bool web_online = false;
char incomingPacket[255]="Umeko Lab";
char lastFrom[15]="Serial 0";
const char *now_IP;

#if defined(HAVE_OLED)
// #include <Wire.h>
#include <U8g2lib.h>
#include "oledDisplay.h"
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ I2C_SCL, /* data=*/ I2C_SDA);
#endif
#if defined(HAVE_INA226)
PowerManager PM1(INA_ADDR1);
PowerManager PM2(INA_ADDR2);
#endif
TaskHandle_t restartTaskHandle;

#if defined(USE_WIFI)
#include <WiFiUdp.h>
#include "wifi_communicate.h"
#include "web_page.h"
// 定义任务句柄，可以通过句柄与其它任务通信
TaskHandle_t wifiStartupTaskHandle;
TaskHandle_t udpCmdTaskHandle;

TaskHandle_t webServerHandle;

WiFiUDP udp1;
UDP_Communicator udp_com(&udp1);
QuePrinter web_printer;

void wifi_startup_task(void* parameter) {
    TickType_t xStartTime = xTaskGetTickCount();
    const TickType_t xWait = 5000;
    while(xStartTime + xWait > xTaskGetTickCount()){
        if(udp_com.wifi_startup()){
            web_online = true;
            led_controller.setPeriod(1000);
            now_IP = strdup(WiFi.localIP().toString().c_str());
            break;
        }
        vTaskDelay(10);
    }
    if (!web_online){
        WiFi.softAP(APssid, APpassword);
        Serial.println("Faild to connect Wifi.");
        Serial.printf("AP started at: \"%s\"\n", APssid);
        Serial.print("UDP server started at ");
        Serial.print(WiFi.softAPIP());
        Serial.printf(":%d\n", COMMUNICATE_PORT);
        now_IP = strdup(WiFi.softAPIP().toString().c_str());
        strcpy(udp_com.local_IP, now_IP);
        web_online = true;
    }
    xTaskNotifyGive(udpCmdTaskHandle);
    xTaskNotifyGive(webServerHandle);
    vTaskDelete(NULL); 
}

void task_UpdCmd(void * ptr){
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    udp_com.broadcast_string("Surgerobot Online!");
    // log_board_msg(&udp_com);
    udp_com.printf("Servo_bus:\n\tservo_1 :\t%d\tservo_2 :\t%d\n", uservo0.isOnline, uservo1.isOnline);
    led_controller.setMode(FADE);
    for(;;){
        int packetSize = udp1.parsePacket();
        if (packetSize) {
            int len = udp1.read(incomingPacket, 255);
            if (len > 0) {
            incomingPacket[len] = 0;
            }
            strcpy(lastFrom, udp1.remoteIP().toString().c_str());
            Serial.print("Received packet from ");
            Serial.print(udp1.remoteIP());
            Serial.print(": \"");
            Serial.print(incomingPacket);
            Serial.println("\"");
            reqHandler(String(incomingPacket), &udp_com);
        }
        vTaskDelay(10);
    }
}



void web_task(void * ptr){
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    init_web(now_IP);
    for(;;){
        server_handleClient();
        if(strlen(text_msg_str) != 0){
            // flag_fresh_oled = true;
            // sprintf(incomingPacket, "%s", text_msg_str);
            strcpy(incomingPacket, text_msg_str);
            strcpy(lastFrom, web_remote_IP);
            strcpy(text_msg_str, "");
            reqHandler(String(incomingPacket), &web_printer);
        }
        web_printer.update();
        vTaskDelay(10);
    }
}
#endif

// 当持续一段时间没有接收到上位机指令，直接停止所有电机运行
void task_auto_break(void * ptr){
    while (true)
    {
        if (LastIncomeMessageTime+AUTO_BREAK_TICKS < xTaskGetTickCount()){
            motor_0.motorSpeedPID(0.0);
            motor_1.motorSpeedPID(0.0);
            servo_spd(0.0, &uservo0);
            servo_spd(0.0, &uservo1);
            LastIncomeMessageTime = xTaskGetTickCount();
            // if (SERIAL_LOG) {
            //     Serial.printf("\nAuto Break After %d mils!\n", AUTO_BREAK_TICKS);
            // }
        }
        vTaskDelay(50);
    }
}

// 检查舵机的运转情况
void check_servo_bus(void * ptr){
    while (true){
        uservo0.ping();
        uservo1.ping();
        uservo0.voltage =(float)uservo0.queryVoltage()/1000;
        uservo0.current =(float)uservo0.queryCurrent()/1000;
        uservo0.power   =(float)uservo0.queryPower()/1000;
        uservo1.voltage =(float)uservo1.queryVoltage()/1000;
        uservo1.current =(float)uservo1.queryCurrent()/1000;
        uservo1.power   =(float)uservo1.queryPower()/1000;
        vTaskDelay(SERVO_CHECK_INTERVEL);
    } 
}

// 软重启任务
void task_restart(void * ptr){
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = 1000;
    vTaskDelayUntil(&xLastWakeTime, xFrequency);
    motor_0.motorSpeed(0.0);
    motor_1.motorSpeed(0.0);
    servo_spd(0.0, &uservo0);
    servo_spd(0.0, &uservo1);
    ESP.restart();
}

#if defined(HAVE_OLED)
void task_u8g2(void * ptr){
    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.clearBuffer();
    #if defined(USE_WIFI)
    connecting_page[1] = new char[20];
    sprintf(connecting_page[1], "SSID: %s", udp_com.get_ssid());
    change_page(&u8g2, blank_page, connecting_page);
    // u8g2.drawStr(10, 30, "Wifi connecting ...");
    // u8g2.sendBuffer();
    // vTaskDelay(5000);
    while (!web_online){ // 等待wifi结果
       vTaskDelay(10);
    }
    
    if(udp_com.connected){
        connecting_page[2] = "Connected!";
        connecting_page[3] = new char[20];
        sprintf(connecting_page[3], "IP: %s", WiFi.localIP().toString().c_str());
        // u8g2.drawStr(20, 40, WiFi.localIP().toString().c_str());
    }else{
        connecting_page[2] = "Failed!";
        connecting_page[3] = new char[25];
        // u8g2.drawStr(30, 40, WiFi.softAPIP().toString().c_str());
        sprintf(connecting_page[3], "Start AP; IP: %s", WiFi.softAPIP().toString().c_str());
    }
    render_page(&u8g2, connecting_page, 0, 0);
    u8g2.sendBuffer();
    vTaskDelay(1000);
    #endif // #if defined(USE_WIFI)
    robo_init_display(&u8g2, uservo0.isOnline, uservo1.isOnline);
    vTaskDelay(1000);
    #if defined(HAVE_INA226)
    motor_stat_display(&u8g2, &motor_0, &motor_1, &uservo0, &uservo1, &PM1, &PM2, incomingPacket);
    #else
    led_controller.setMode(FADE);
    motor_stat_display(&u8g2, &motor_0, &motor_1, &uservo0, &uservo1, incomingPacket);
    #endif
}
#endif

void task_blink(void * ptr){
    led_controller.setMode(BLINK);
    for(;;){
        led_controller.mainloop();
    }
}

void task_distanceCounter(void * ptr){
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = 2;
    const float dt = xFrequency * 0.001;       // 2ms计算一次电机位置
    while (true){
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
        motor_0.updateDistance(dt, false);
        motor_1.updateDistance(dt, false);
    }  
}

void task_lim_sensor(void * ptr){
    motor_0.strokeInit(LIM_SENSOR_PIN_1, LIM_SENSOR_PIN_2, LIM_SENSOR_PIN_1_TRIG_LEVEL); // 限位开关初始化
    while (true){
        motor_0.loop_strokeChecker();
        // motor_1.loop_strokeChecker();
        vTaskDelay(10);
    }
}

void motor_control(void * ptr){
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = DT;
    for(;;){
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
        // float old_output1 = motor_0.pid_conroller->output;
        float output1 = motor_0.pid_conroller->step(motor_0.targetSpeed);
        // float old_output2 = motor_1.pid_conroller->output;
        float output2 = motor_1.pid_conroller->step(motor_1.targetSpeed);
        // Serial.printf("%.2f\t%.2f\n", old_output, output);
        motor_0.motorSpeed(output1);
        motor_1.motorSpeed(output2);
    }
}

// void motor0_control(void * ptr){
//     motor_0.loop_runner();
// }
// void motor1_control(void * ptr){
//     motor_1.loop_runner();
// }

void task_req_analyze(void * ptr){
    String req;
    #if defined(BOARD_TYPE_D4)
    String req_2;
    #endif
    while (true){
        req = "";
        if (Serial.available()){
            req = Serial.readStringUntil('\n');
            reqHandler(req, &Serial);
            strcpy(incomingPacket, req.c_str());
            strcpy(lastFrom, "Serial0");
        }
        #if defined(BOARD_TYPE_D4)
        req_2 = "";
        if (Serial2.available()){
            req = Serial2.readStringUntil('\n');
            reqHandler(req, &Serial2);
            strcpy(incomingPacket, req.c_str());
            strcpy(lastFrom, "Serial1");
        }
        #endif
    vTaskDelay(10);
    }
}

void reqHandler(String req, Print * printer){
    bool valid = false;
    if (req.substring(0, 1) == ":"){
        uint8_t motorId = req.substring(1).toInt();
        double spd = req.substring(3).toFloat();
        if (motorId == 0){
            motor_0.motorSpeedPID(spd);
            valid = true;
        }
        else if (motorId == 1){
            motor_1.motorSpeedPID(spd);
            valid = true;
        }
        else if (motorId == 2){
            servo_spd(spd, &uservo0);
            valid = true;
        }
        else if (motorId == 3){
            servo_spd(spd, &uservo1);
            valid = true;
        }
        // if (valid) {printer->printf("motor %d set %.2f\n", motorId, spd);}
    }else if(req.substring(0, 1) == "!"){
        printer->print(req.substring(1));
        if (req.substring(1)=="RESTART"){
            xTaskNotifyGive(restartTaskHandle);
            printer->printf("\nSystem will be RESTART at %d mils\n", 1000);
        }
    }else if (req.substring(0, 1) == "?"){
        uservo1.queryRawAngleMTurn();
        printer->print("::");
        size64Writer(printer, motor_0.nowDistance);
        size64Writer(printer, uservo1.queryRawAngleMTurn());
        size64Writer(printer, motor_1.nowDistance);
        printer->print("\n");
    }else if (req.substring(0, 1) == "$"){
        uint8_t setType = req.substring(1).toInt();
        if (setType == SET_IP){
            #if defined(USE_WIFI)
            udp_com.set_remote_ip(req.substring(3).c_str());
            #endif
            printer -> printf("RemoteIP set to \"%s\"\n", req.substring(3).c_str());
        }else if(setType == SET_SSID){
            #if defined(USE_WIFI)
            udp_com.set_ssid(req.substring(3).c_str());
            #endif
            printer -> printf("SSID set to \"%s\"\n", req.substring(3).c_str());
        }else if(setType == SET_PASSWORD){
            #if defined(USE_WIFI)
            udp_com.set_password(req.substring(3).c_str());
            #endif
            printer -> printf("PassWord set to \"%s\"\n", req.substring(3).c_str());
        }else if(setType == BROADCAST){
            #if defined(USE_WIFI)
            udp_com.broadcast_string(req.substring(3));
            #endif
        }else if(setType == SENDMSG){
            #if defined(USE_WIFI)
            udp_com.write_string(req.substring(3));
            #endif
        }else if(setType == LOG_WIFI){
            flag_u8g2_show_msg = true;
            #if defined(USE_WIFI)
            udp_com.log_output(printer);
            #endif
            msg_logger(printer);
        }
    }
    if (valid) {
        LastIncomeMessageTime = xTaskGetTickCount();
    }
}

//周期性报告运转信息
void task_msg_logger(void * ptr){
    while (true){
        if(SERIAL_LOG){
            msg_logger(&Serial);
            #if defined(BOARD_TYPE_D4)
            msg_logger(&Serial2);
            #endif
            #if defined(USE_WIFI)
            if(web_online){msg_logger(&udp_com);}
            #endif
        }
        vTaskDelay(LOG_INTERVAL);
    }
}

// 可读性强的机器人运转信息报告
void msg_logger(Print * printer){
    char log_str[256];
    sprintf(log_str, "motor spd:\n\tmotor1 :\t%.2f\tmotor2 :\t%.2f\n", 
                motor_0.nowFreqAbs, motor_1.nowFreqAbs);
    sprintf(log_str, "%s\tmotor3 :\t%.2f\tmotor4 :\t%.2f\n", 
                log_str, uservo0.speed, uservo1.speed);
    sprintf(log_str, "%slim_sensor: \t%d means triggered\n\t%d :\t%d\t%d :\t%d\n",
                log_str, LIM_SENSOR_PIN_1_TRIG_LEVEL, LIM_SENSOR_PIN_1, 
                motor_0.positivePinValue, LIM_SENSOR_PIN_2, motor_0.negativePinValue);
    sprintf(log_str, "%smotor pos:\n\tmotor1 :\t%d",log_str, (int)(motor_0.nowDistance / 1000));
    sprintf(log_str, "%s\tmotor2 :\t%d\n",log_str, (int)(motor_1.nowDistance / 1000));
    #if defined(HAVE_INA226)
    sprintf(log_str, "%spower info:\n\tmotor1 :\t", log_str);
    sprintf(log_str, "%s%.2f V\t%.2f A\t%.2f W\t",
                log_str, PM1.getVoltage(), PM1.getCurrent(), PM1.getPower());
    sprintf(log_str, "%s\n\tmotor2 :\t", log_str);
    sprintf(log_str, "%s%.2f V\t%.2f A\t%.2f W\n",
                log_str, PM2.getVoltage(), PM2.getCurrent(), PM2.getPower());
    #endif
    printer->print(log_str);
    // printer->printf("%d", motor_0.nowDistance);
    // printer->printf("motor spd:\n\tmotor1 :\t%.2f\tmotor2 :\t%.2f\n", 
    //             motor_0.nowFreqAbs, motor_1.nowFreqAbs);
    // printer->printf("\tmotor3 :\t%.2f\tmotor4 :\t%.2f\n", 
    //             uservo0.speed, uservo1.speed);
    // printer->printf("lim_sensor: \t%d means triggered\n\t%d :\t%d\t%d :\t%d\n", LIM_SENSOR_PIN_1_TRIG_LEVEL,
    //             LIM_SENSOR_PIN_1, motor_0.positivePinValue, LIM_SENSOR_PIN_2, motor_0.negativePinValue);
    // printer->printf("motor pos:\n\tmotor1 :\t%d", motor_0.nowDistance);
    // printer->printf("\tmotor2 :\t%d\n", motor_1.nowDistance);
    // #if defined(HAVE_INA226)
    // printer->print("power info:\n\tmotor1 :\t");
    // printer->printf("%.2f V\t", PM1.getVoltage());
    // printer->printf("%.2f A\t", PM1.getCurrent());
    // printer->printf("%.2f W\t", PM1.getPower());
    // printer->printf("\n\tmotor2 :\t%.2f V\t", PM2.getVoltage());
    // printer->printf("%.2f A\t", PM2.getCurrent());
    // printer->printf("%.2f W\t\n", PM2.getPower());
    // #endif
    if(uservo0.isOnline){
        printer->printf("\n\tservo1 :\t%.2f V\t%.2f A\t%.2f W\t\n", uservo0.voltage, uservo0.current, uservo0.power);
        // printer->printf("\n\tservo1 :\t%.2f V\t", uservo0.voltage);
        // printer->printf("%.2f A\t", uservo0.current);
        // printer->printf("%.2f W\t\n", uservo0.power);
    }
    if(uservo1.isOnline){
        printer->printf("\n\tservo2 :\t%.2f V\t%.2f A\t%.2f W\t\n", uservo1.voltage, uservo1.current, uservo1.power);
        // printer->printf("%.2f A\t", uservo1.current);
        // printer->printf("%.2f W\t\n", uservo1.power);
    }
}

void setup() {
    protocol.init(TX_2, RX_2);        // 通信协议初始化
    
    // motor_1.strokeInit(LIM_SENSOR_PIN_1, LIM_SENSOR_PIN_2, LIM_SENSOR_PIN_1_TRIG_LEVEL); // 限位开关初始化
    uservo0.init();          //舵机0角度初始化
    uservo1.init();          //舵机1角度初始化
    uservo0.ping();
    uservo1.ping();

    Serial.begin(115200);
    Serial.setTimeout(5);
    log_board_msg();
    Serial.printf("Servo_bus:\n\tservo_1 :\t%d\tservo_2 :\t%d\n", uservo0.isOnline, uservo1.isOnline);

    #if defined(BOARD_TYPE_D4)
    Serial2.begin(115200, SERIAL_8N1, RX_1, TX_1);
    log_board_msg(&Serial2);
    Serial2.printf("Servo_bus:\n\tservo_1 :\t%d\tservo_2 :\t%d\n", uservo0.isOnline, uservo1.isOnline);
    #endif

    #if defined(HAVE_INA226)
    PM1.setWire(I2C_SDA, I2C_SCL);
    PM2.setWire(I2C_SDA, I2C_SCL);
    #endif

    #if defined(USE_WIFI)
    udp_com.load_SSID();
    udp_com.load_PSWD();
    udp_com.load_rIP();
    xTaskCreate(wifi_startup_task, "START_WIFI", 1024 * 3, NULL, 1, &wifiStartupTaskHandle);
    xTaskCreate(task_UpdCmd, "UDP_CMD", 1024 * 3, NULL, 2, &udpCmdTaskHandle);
    xTaskCreate(web_task,   "WEBSERVER",1024 * 6,     NULL, 2, &webServerHandle);
    #endif

    //  ------------TASKS----------------
    xTaskCreate(task_req_analyze, "REQ_ANALYZE",     1024 * 6, NULL, 2, NULL);
    xTaskCreate(task_auto_break, "AUTO_BREAK",       1024 * 2, NULL, 1, NULL);
    xTaskCreate(task_distanceCounter, "DIS_COUNTER", 1024 * 2, NULL, 1, NULL);
    xTaskCreate(task_lim_sensor, "LIM_SENSOR",       1024,     NULL, 2, NULL);
    xTaskCreate(motor_control, "MOTOR_0_CTRL",       1024* 2,     NULL, 2, NULL);
    // xTaskCreate(motor1_control, "MOTOR_1_CTRL",       1024* 2,     NULL, 1, NULL);
    // xTaskCreate(task_msg_logger, "MSG_LOGGER",       1024 * 4, NULL, 2, NULL);
    xTaskCreate(task_blink, "ONBOARD_LED_BLINK",     1024,     NULL, 2, NULL);
    xTaskCreate(task_restart, "RESTART",             1024 * 2,     NULL, 1, &restartTaskHandle);
    xTaskCreate(check_servo_bus, "CHECK_SERVO",      1024,     NULL, 1, NULL);

    #if defined(HAVE_OLED)
    u8g2.begin();
    xTaskCreate(task_u8g2, "OLED", 1024 * 10, NULL, 2, NULL);
    #endif
}

void loop() {
    // Serial2.printf("motor pos:\n\tmotor1 :\t%d\tmotor2 :\t%d\t\n", 
    //             motor_0.nowDistance, motor_1.nowDistance);
    // Serial2.println(motor_1.nowDistance);
    // vTaskDelay(10000);
    // flag_u8g2_show_msg = true;
}
